import lejos.nxt.*;
import lejos.robotics.localization.*;
import lejos.robotics.navigation.*;

public class Scanbot {

  UltrasonicSensor usensor;
  DifferentialPilot pilot;
  Navigator navi;
  PoseProvider pose;
  MapScanner scanner;
  
  public Scanbot() {
    pilot = new DifferentialPilot(5.6f, 12.f, Motor.A, Motor.B);
    pilot.setRotateSpeed(30);
    pilot.setTravelSpeed(10);
    usensor = new UltrasonicSensor(SensorPort.S1);
    usensor.setMode(UltrasonicSensor.MODE_CONTINUOUS);
    navi = new Navigator(pilot);
    pose = navi.getPoseProvider();
    
  }
  
  public void scanSquare(int width) {
    scanner = new MapScanner(pose, usensor, width);
    float realwidth = (float) width*2;
    navi.addWaypoint(realwidth, 0.f);
    navi.addWaypoint(0.f, 0.f);
    navi.addWaypoint(realwidth/2, 0.f);
    navi.addWaypoint(realwidth/2, -realwidth/2);
    navi.addWaypoint(realwidth/2, realwidth/2);
    navi.addWaypoint(realwidth/2, 0);
    Thread scanThread = new Thread(scanner);
    scanThread.start();
    navi.followPath();
    //navi.waitForStop();
  }
  
  public static void main(String args[]) {
    int width = 45;
    float thresh = 0.5f;
    Scanbot bot = new Scanbot();
    float map[][];
    bot.scanSquare(width);
    LCD.clearDisplay();
    while (true) {
      for (int x = 0; x < width; ++x)
	for (int y = 0; y < width; ++y)
	  LCD.setPixel(x, y, bot.scanner.map[x][y] > thresh ? 1 : 0);
    }
  }

}